/*************************************************************
*
* M3D_SPH.CPP
*
* Sphere functionality
*************************************************************/
#include <stdafx.h>

//#include <ap_comm.h>
#include <m3d/m3d.h>

/*************************************************************
* m3dSPHERE::IsBelongPoint()
*
*************************************************************/
BOOL m3dSPHERE::IsBelongPoint (const m3dV *v)
{
   if (m3dDist_2(&org, v) > m3dPow2(radius)) {
      return FALSE;
   }
   return TRUE;
}
/*************************************************************
* m3dBOX::IsectCyl ()
*    Test isection of cylynder and sphere
*************************************************************/
BOOL m3dSPHERE::IsectCyl (m3dV *vFrom, m3dV *dir, float maxDist, float radius, m3dV *vClosest, float *dist, int *faceNmb)
{
   int rc;

   m3dSPHERE::Expand(radius);
   rc = m3dSPHERE::IsectRay(vFrom, dir, maxDist, vClosest, dist, faceNmb);
   m3dSPHERE::Expand(-radius);
   
   return rc != M3D_ISECT_NO;
}

/*************************************************************
* m3dSPHERE::IsectRay()
*
* Intersect ray with the sphere
*
* IN : rayOrg, rayD   - ray origin/direction
*      sphOrg, radius - sphere origin/radius
* OUT: vClosest       - closest point of intersection (can be NULL)
*      dist           - distance from rayOrg and vClosest
* RET: M3D_ISECT_BELONG - rayOrg belongs to the sphere
*      M3D_ISECT_YES    - ray intersects the sphere (point of intersection is found)
*      M3D_ISECT_NO     - ray does not intersect the sphere (point of intersection is not found)
*
* NOTE:
*         (a, rayDir) +- sqrt((a, rayDir)^2 - rayDir^2(a^2 - r^2))
*  t  =  ----------------------------------------------------------
*                            rayDir^2
*
*  p  = rayOrg + rayDir * t
*************************************************************/
int m3dSPHERE::IsectRay (m3dV *rayOrg, m3dV *rayDir, float distMax,
                         m3dV *vClosest, float *dist, int *faceNmb)
{
   float ra2/*, rd2, ra_dotprod_rd, t1, t2, det, det2*/;
   float t;
   m3dV  a;
   m3dV  vCl;
   float d;

   ASSERT(rayOrg != NULL);
   ASSERT(rayDir != NULL);

   ASSERT(Check());

   // see formulae
   m3dSubtractVector(rayOrg, &org, &a);

   ra2 = m3dLengthVector_2(&a);
   
   // test, if the rayOrg belong to BSphere, and if it is TRUE, then get out
   if (ra2 < radius * radius)   {
      if (dist) {
         *dist = 0.f;
      }
      if (vClosest) {
         *vClosest = *rayOrg;
      }
      return M3D_ISECT_BELONG;
   }

   int rc = _IsectRay(rayOrg, rayDir, &t);
   if (rc != M3D_ISECT_YES) { 
      return rc;
   }

   // calculate vClosest
   _m3dLineCombine(rayOrg, rayDir, t, &vCl);
   if (vClosest) {
      *vClosest = vCl;
   }
   d = m3dDist(&vCl, rayOrg);
   if (d > distMax) {
      return M3D_ISECT_NO;
   }

   if (dist)  {
      *dist = d;
   }

   if (faceNmb) {
      *faceNmb = -1;
   }

   return M3D_ISECT_YES;
}
/*************************************************************
* m3dSPHERE::IsectRay()
*
* Intersect ray with the sphere
*
* IN : rayOrg, rayD   - ray origin/direction
*      sphOrg, radius - sphere origin/radius
* OUT: vClosest       - closest point of intersection (can be NULL)
*      dist           - distance from rayOrg and vClosest
* RET: M3D_ISECT_BELONG - rayOrg belongs to the sphere
*      M3D_ISECT_YES    - ray intersects the sphere (point of intersection is found)
*      M3D_ISECT_NO     - ray does not intersect the sphere (point of intersection is not found)
*
* NOTE:
*         (a, rayDir) +- sqrt((a, rayDir)^2 - rayDir^2(a^2 - r^2))
*  t  =  ----------------------------------------------------------
*                            rayDir^2
*
*  p  = rayOrg + rayDir * t
*************************************************************/
int m3dSPHERE::_IsectRay (m3dV *rayOrg, m3dV *rayDir, float *t)
{
   float ra2, rd2, ra_dotprod_rd, t1, t2, det, det2;
   m3dV  a;
   //m3dV  vCl;
   //float d;

   // see formulae
   m3dSubtractVector(rayOrg, &org, &a);

   ra2 = m3dLengthVector_2(&a);

   rd2           = m3dLengthVector_2(rayDir);
   ra_dotprod_rd = m3dDotProduct(&a, rayDir);

   det2 = ra_dotprod_rd * ra_dotprod_rd - rd2 * (ra2 - radius * radius);
   if (det2 < 0.f) {
      // intersection does not exist
      return M3D_ISECT_NO;
   }

   det = m3dSqrt(det2);
   t1  = (-ra_dotprod_rd + det) / rd2;
   t2  = (-ra_dotprod_rd - det) / rd2;

   // select minimal T, T > 0
   if (t1 < 0)   {
      if (t2 < 0)   {
         return M3D_ISECT_NO;
      }
      *t = t2;
   }  else  {
      if (t2 > 0)   {
         // t1, t2 > 0
         *t = min(t1, t2);
      } else {
         *t  = t1;
      }
   }
   return M3D_ISECT_YES;
}

/*************************************************************
* m3dSPHERE::DistPoint()
* 
* Calculate distance between point and sphere
*
* IN : p        - point
* OUT: vClosest - nearest point on bounding prizm (may be NULL)
*      dist     - distance to nearest point (may be NULL)
* RET: M3D_DIST_FOUND/M3D_DIST_NOTFOUND/M3D_DIST_BELONG
*                     vClosest & dist are not filled
*
* NOTE: if 'p' belongs to pBSph, vClosest/dist are not filled
*************************************************************/
int m3dSPHERE::DistPoint (const m3dV *p, float distMax, m3dV *vClosest, float *dist, int *faceNmb) const
{
   // Calculate distance between point & sphere origin
   float orgDist2 = m3dDist_2(p, &org);
   if (orgDist2 <= radius * radius) {
      return M3D_DIST_BELONG;
   }

   if (orgDist2 > (distMax + radius) * (distMax + radius)) {
      return M3D_DIST_NOTFOUND;
   }

   float orgDist= m3dSqrt(orgDist2);
   
   if (dist) {
      *dist = orgDist - radius;
   }
   
   // Calculate vClosest
   if (vClosest) {
      float a = radius / orgDist;
      vClosest->x = org.x + a * (p->x - org.x);
      vClosest->y = org.y + a * (p->y - org.y);
      vClosest->z = org.z + a * (p->z - org.z);
   }
   
   if (faceNmb) {
      *faceNmb = -1;
   }
   return M3D_DIST_FOUND;
}

/*************************************************************
* m3dSPHERE::Check()
*
*************************************************************/
BOOL m3dSPHERE::Check (void) const
{
   if (radius > M3D_EPSILON) {
      return TRUE;
   }
   return FALSE;
}

/*******************************************************
* m3dSPHERE::CalcEnclSph()
*
*******************************************************/
void m3dSPHERE::CalcEnclSph (m3dSPHERE *pSph)
{
   *pSph = *this;
   return ;
}

/*******************************************************
* m3dSPHERE::CheckEnclSph()
*
*******************************************************/
BOOL m3dSPHERE::CheckEnclSph (m3dSPHERE *pSph)
{
   if (!m3dIsEqualPoint_A(&pSph->org, &org, 10.f*M3D_EPSILON)) {
      return FALSE;
   }
   if (m3dFAbs(pSph->radius - radius) > 10.f*M3D_EPSILON) {
      return FALSE;
   }
   return TRUE;
}

/*************************************************************
* m3dSPHERE::Calc()
*
* IN: pointList - array of points
*     n         - number of points in the list
*
* OUT: sphere (this) enclosing point list
*
* Approximate algorithm is being used.
* The diameter of the point set is found, and then
* the sphere is constructed on the center of this diameter
*************************************************************/
void m3dSPHERE::Calc (m3dV *pointList, int nPoint)
{
   int    n1 = 0, n2 = 0, i;
   float  radius_2;
   
   ASSERT(nPoint > 0);
   _FindMaxLSeg(pointList, nPoint, &n1, &n2);
   m3dLineCombine(&pointList[n1], 0.5f, &pointList[n2], 0.5f, &org);
   radius_2 = m3dDist_2(&pointList[n1], &org);
   for (i = 0; i < nPoint; i++) {
      radius_2 = max(radius_2, m3dDist_2(&org, &pointList[i]));
   }
   radius = m3dSqrt(radius_2) + 100.f*M3D_EPSILON;

#ifdef _DEBUG   
   // check that all the points are inside the sphere
   for (i = 0; i < nPoint; i++) {
      if (!IsBelongPoint(&pointList[i])) {
         ASSERT(FALSE);
      }
   }
#endif
   return;
}


/************************************************************
* m3dSPHERE::Expand()
*
************************************************************/
void m3dSPHERE::Expand (float value)
{
   radius += value;
   return;
}
/************************************************************
* m3dSPHERE::QualPlane ()
*
************************************************************/
int m3dSPHERE::QualPlane (m3dV *planeOrg, m3dV *planeNorm)
{
   float dist; 
   
   dist = m3dDistPointPlane_S(&org, planeOrg, planeNorm);

   if (m3dFAbs(dist) <= radius) {
      return M3D_ISECT;
   }
   if (dist < 0.f) {
      return M3D_OUTSIDE;
   }
   return M3D_INSIDE;
}



/*************************************************************
* m3dSPHERE::_FindMaxLSeg()
*
* IN:   pointList - array of points
*       nPoint    - number of points in the list
* OUT:  n1, n2 - indices of points with maximal distance
* NOTE: this function can be optimized!!!
*************************************************************/
void m3dSPHERE::_FindMaxLSeg (m3dV *pointList, int nPoint, int *n1, int *n2)
{
   int    i,j;
   float  maxDist_2 = -1.f;
   float  dist_2;

   ASSERT(nPoint > 0);

   if (nPoint == 1) {
      // degenerate case: one point only
      *n1 = 0;
      *n2 = 0;
      return;
   }

   for (i = 0; i < nPoint; i++) {
      for (j = i+1; j < nPoint; j++) {
         dist_2 = m3dDist_2(&pointList[i], &pointList[j]);
         if (dist_2  > maxDist_2) {
            *n1 = i;
            *n2 = j;
            maxDist_2 = dist_2;
         }
      }
   }
   return;
}

//
// End-of-file M3D_SPH.CPP
//
